

#include "MyProject.h"


/******************************************************************************/
MagnetSensor_Para  AS5047P_para,TLE5012B_para,MA730_para;
/******************************************************************************/
long  cpr;
float full_rotation_offset;
long  angle_data_prev;
float angle_prev;
unsigned long velocity_calc_timestamp;
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
float getAngle(void)
{
	
	return (float)MT6825_ReadAngle() * _2PI / 262144.0f;
}
/******************************************************************************/
// Shaft velocity calculation
float getVelocity(void)
{
	unsigned long now_us;
	float Ts, angle_now, vel;

	// calculate sample time
	now_us = SysTick->VAL; //_micros();
	if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
	else
		Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;
	// quick fix for strange cases (micros overflow)
	if(Ts == 0 || Ts > 0.5) Ts = 1e-3;

	// current angle
	angle_now = getAngle();
	// velocity calculation
	vel = (angle_now - angle_prev)/Ts;

	// save variables for future pass
	angle_prev = angle_now;
	velocity_calc_timestamp = now_us;
	return vel;
}
/******************************************************************************/


